from turtle import color
from location_init import *
import math,time

import matplotlib.pyplot as plt
import position_config as pc
from matplotlib.animation import FuncAnimation
from matplotlib.patches import Rectangle

from position import Position
from distance import Distance

def raise_loction_display(proc_name,loc_server,simulate_map):
    print("Proc [{}] start".format(proc_name))
    global grd_data_source,sim_data_source
    global location_server
    location_server = loc_server

    global sim_map
    sim_map = simulate_map

    # grd_data_source = loc_server.get_grd_display_info
    # sim_data_source = loc_server.get_sim_display_info
    raise_plt()
    print("Proc [{}] end".format(proc_name))

def basic_set(ax,xlim_range,ylim_range,title_str):
    ax.set_xlabel("x")
    ax.set_ylabel("y")
    ax.set_aspect('equal')
    ax.set_xlim([0,xlim_range])
    ax.set_ylim([0,ylim_range])
    ax.set_title(title_str)

def init_guard_ax_set(ax):
    xlim_range = pc.E
    ylim_range = pc.E
    title_str = "Grd"
    basic_set(ax,xlim_range,ylim_range,title_str)

def init_simulate_ax_set(ax):
    global sim_map
    xlim_range = sim_map.E()
    ylim_range = sim_map.E()
    title_str = "Sim"
    basic_set(ax,xlim_range,ylim_range,title_str)
    
    # TODO draw map function
    ws = sim_map.walls()
    # print(ws)
    for w in ws:
        x,y,xl,yl = w
        ax.add_patch(Rectangle((x, y), xl, yl,color="red"))


def raise_plt():
    global axes
    fig, axes = plt.subplots(1, 2, figsize=(pc.plt_size*2,pc.plt_size))
    axes = axes.ravel()

    global ax_color 
    ax_color = [
        {'color':'b','arrow_color' : 'k','marker':'o'},
        {'color':'r','arrow_color' : 'k','marker':'o'}
    ]

    ani_grd = FuncAnimation(fig=fig, func=animate_update_grd, frames=10)
    ani_sim = FuncAnimation(fig=fig, func=animate_update_sim, frames=10)
    plt.show()

def create_pos_fig(ax,pos,color = 'b',arrow_color = 'k',
    marker='o',is_latest=False,aw_len=50):
    if (ax is None): return 
    '''
    return (scatter, arrow) to show a car's position
    '''
    # assert(isinstance(pos,Position))
    x,y,rad = pos['x'],pos['y'],pos['rad']
    # print("pos = ",pos)
    dx = aw_len*math.cos(rad)
    dy = aw_len*math.sin(rad)

    if (not is_latest):
        color = 'gray'
        pt = ax.scatter([x],[y],
                        s=50,c=color,marker=marker,alpha=0.5)
    else:
        pt = ax.scatter([x],[y],
                        s=50,c=color,marker=marker,alpha=0.5)
        pt_aw =ax.arrow(x,y,dx,dy,color=arrow_color,width=1)


def animate_update_grd(fid):
    global axes
    ax = axes[0]

    global location_server
    if (location_server.is_shutdown()): return
    
    loc_list = location_server.get_grd_display_info(5)
    ax.clear()
    init_guard_ax_set(ax)

    for i in range(1,len(loc_list)):
        create_pos_fig(ax,loc_list[i],is_latest=False)
    create_pos_fig(ax,loc_list[0],is_latest=True,aw_len=50)


# different from guard, need to build simulate map
def animate_update_sim(fid):
    global axes
    ax = axes[1]

    global location_server
    if (location_server.is_shutdown()): return
    
    # get car center
    loc_list = location_server.get_sim_display_info(5)
        # location_server.get_sim_display_info(5)
    ax.clear()
    init_simulate_ax_set(ax)

    for i in range(1,len(loc_list)):
        create_pos_fig(ax,loc_list[i],is_latest=False)
    create_pos_fig(ax,loc_list[0],is_latest=True,aw_len=50)

    irs_pos = Position(loc_list[0],is_irs_center=False)._calc_sensor_center()

    # TODO 画传感器辅助线
    global sim_map
    status,inter_md_dis,inter_ps = sim_map.get_inter_info()
    if (status):
        # draw_distance_ray(ax,inter_ps,irs_pos.x,irs_pos.y)
        show_distance_txt(ax,inter_md_dis)
    
    # print("sim_ani end")

def draw_distance_ray(ax,inter_ps,irs_x,irs_y):
    first = True
    for p in inter_ps:
        if not first:
            co = 'black'
        else:
            co = 'red'
            first = False
        ax.arrow(irs_x,irs_y,p[0]-irs_x,p[1]-irs_y,color=co)
    pass

def show_distance_txt(ax,inter_dis):
    F,B,L,R = inter_dis[:4]
    dis_str = "F:{:.3f}\nB:{:.3f}\nL:{:.3f}\nR:{:.3f}\n".format(
        F,B,L,R
    )
    txt = ax.text(fontsize=6,x=50,y=50,s=dis_str,
        verticalalignment='bottom',horizontalalignment='left')
